#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include "eigen3/Eigen/Dense"
#include "opencv2/core/eigen.hpp"
#include<vector>
using namespace std;
class average_filter{
    private:
    int n;
    vector<sensor_msgs::Imu> m;
    public:
    average_filter();
    ~average_filter();
    void set_num(int num);
    int get_n();
    void get_msgs(sensor_msgs::Imu msg);
    sensor_msgs::Imu get_average();
};
class subandpub{
    private:
    average_filter filter;
    int count=0;
    ros::NodeHandle n;
    ros::Subscriber imu_info_sub;
    ros::Publisher IMU_average_pub;
    public:
    subandpub();
    ~subandpub();
    void callback(const sensor_msgs::Imu::ConstPtr& msg);
};

//函数实现
average_filter::average_filter(){
}
average_filter::~average_filter(){
}
void average_filter::set_num(int num){
    n=num;
}
int average_filter::get_n(){
    return n;
}
//待补全
void average_filter::get_msgs(sensor_msgs::Imu M){
    m.push_back(M);
}
//待补全
sensor_msgs::Imu average_filter::get_average(){
    sensor_msgs::Imu filtered;
    double x_acc=0;
    double y_acc=0;
    double z_acc=0;
    for(int i=0;i<n;i++){
        x_acc+=m[i].linear_acceleration.x;
        y_acc+=m[i].linear_acceleration.y;
        z_acc+=m[i].linear_acceleration.z;
    }
    filtered.linear_acceleration.x=x_acc/n;
    filtered.linear_acceleration.y=y_acc/n;
    filtered.linear_acceleration.z=z_acc/n;
    filtered.header = m[2].header;
    m.clear();
    return filtered;
}


subandpub::subandpub(){
    int num;
    std::cout<<"请输入num"<<std::endl;
    std::cin>>num;
    filter.set_num(num);
    subandpub::imu_info_sub = n.subscribe("/imu/data_raw", 10,&subandpub::callback, this);
    subandpub::IMU_average_pub = n.advertise<sensor_msgs::Imu>("/imu_average", 10);
}
subandpub::~subandpub(){
}
void subandpub::callback(const sensor_msgs::Imu::ConstPtr& msg){
    if(count<filter.get_n()){//获取数据
    sensor_msgs::Imu M;
    M.linear_acceleration.x =  msg->linear_acceleration.x;
    M.linear_acceleration.y =  msg->linear_acceleration.y;
    M.linear_acceleration.z =  msg->linear_acceleration.z;
    M.header = msg->header;
       filter.get_msgs(M);
       count++;
   }else{
       IMU_average_pub.publish(filter.get_average());
       count=0;
   }
}
int main(int argc,char** argv){
  ros::init(argc, argv, "subscriber_and_publisher");
  subandpub AverageObject;
  ros::spin();
  return 0;
}


